/*matrix_operation.c*/

#include "kalman_my.h"
#include "matrix_operation.h"

extern matrix v, m3, m4, l, m, n, u, v, x, z, inv;
extern int i, j, k, flag, iter;
extern float sucet, ratio, a;
extern float cell;

// SUCET DVOCH MATIC M + N, vysledok sa ulozi do ADD
int mat_add(matrix *mp, matrix *np, matrix *add) {
  m = *mp;
    n = *np;   

	n.rows=(n.to[0]-n.from[0]);
	n.columns=(n.to[1]-n.from[1]);
	m.rows=(m.to[0]-m.from[0]);
	m.columns=(m.to[1]-m.from[1]);

    if (m.rows == n.rows && m.columns == n.columns) {
        
        //matrix v;
        v.columns = m.columns;
        v.rows = m.rows;
          
        for (j = 0; j < m.rows; ++j) {            
            for (k = 0; k < m.columns; ++k) {
                v.values[j][k] = m.values[j+m.from[0]][k+m.from[1]] + n.values[j+n.from[0]][k+n.from[1]];
            }
        }
        v.from[0]=0; v.from[1]=0;
		v.to[0]=m.rows; v.to[1]=m.columns;
        *add = v;
        return 0;
    }     
    else {
        return 1;
    }
}

// ROZDIEL DVOCH MATIC M - N, vysledok sa ulozi do SUB
int mat_sub(matrix *mp, matrix *np, matrix *sub) {
  m = *mp;
    n = *np;   

	n.rows=(n.to[0]-n.from[0]);
	n.columns=(n.to[1]-n.from[1]);
	m.rows=(m.to[0]-m.from[0]);
	m.columns=(m.to[1]-m.from[1]);

    if (m.rows == n.rows && m.columns == n.columns) {
        
        //matrix v;
        v.columns = m.columns;
        v.rows = m.rows;
          
        for (j = 0; j < m.rows; ++j) {            
            for (k = 0; k < m.columns; ++k) {
                v.values[j][k] = m.values[j+m.from[0]][k+m.from[1]] - n.values[j+n.from[0]][k+n.from[1]];
            }
        }
        v.from[0]=0; v.from[1]=0;
		v.to[0]=m.rows; v.to[1]=m.columns;
        *sub = v;
        return 0;
    }
    else {
        return 1;     
    }
}

// SUCin DVOCH MATIC M * N, , vysledok sa ulozi do MUL
int mat_mul(matrix *mp, matrix *np, matrix *mul) {
  m = *mp;
    n = *np;   
    
    n.rows=(n.to[0]-n.from[0]);
	n.columns=(n.to[1]-n.from[1]);
	m.rows=(m.to[0]-m.from[0]);
	m.columns=(m.to[1]-m.from[1]);
	if (m.columns == n.rows) {
       v.columns = n.columns;
	   v.rows = m.rows;

       
       for (i = 0; i < m.rows; ++i) {
           for (j = 0; j < n.columns; ++j) {
               for (k = 0; k < m.columns; ++k) {                   
                                    cell += m.values[i+m.from[0]][k+m.from[1]] * n.values[k+n.from[0]][j+n.from[1]];
               }
               v.values[i][j] = cell;
               cell = 0;    
           }           
       }		  
	   v.from[0]=0;
	   v.from[1]=0;
	   v.to[0]=m.rows;
	   v.to[1]=n.columns;
       *mul = v;
       return 0;              
    }
    else {
       return 1;
    }    
}

// NASOBENIE MATICE KONSTANTOU M .* n, vysledok sa ulozi do SUB
int mat_mul_const(matrix *mp, float nn, matrix *sub) {
        m = *mp;
		m.rows=(m.to[0]-m.from[0]);
		m.columns=(m.to[1]-m.from[1]);
        v.columns = m.columns;
        v.rows = m.rows;
          
        for (j = 0; j < v.rows; ++j) {
            
            for (k = 0; k < v.columns; ++k) {
                v.values[j][k] = m.values[j][k] * nn;
            }
        }
        v.from[0]=0;
	   	v.from[1]=0;
	   	v.to[0]=m.rows;
	   	v.to[1]=m.columns;
        *sub = v;
        return 0;
    
}

// LU DEKOMPOZICIA M = L * U
int ludec(matrix *mp, matrix *l_out, matrix *u_out) {
  m = *mp;
      
    if (m.rows == m.columns) {
       
       l.rows = m.rows;
       l.columns = m.columns;
       
            // vypocet L
       for (i = m.from[0]; i < m.to[0]; ++i) {
           for (j = m.from[1]; j < m.to[1]; ++j) {
              if (i == j) {
                  l.values[i][j] = 1;
              }
              else {
                  l.values[i][j] = 0;      
              }
           }
       }

       // vypocet U
             j = 0;
       for (j = m.from[1]; j < m.to[1] - 1; ++j) {
           for (i = j+1; i < m.to[0]; ++i) {
               
                            l.values[i][j] = m.values[i][j] / m.values[j][j];               
               for (k = j; k < m.to[1]; ++k) {
                   if ( k != j) {
                      m.values[i][k] = m.values[i][k] - l.values[i][j]*m.values[j][k];
                   }
                   else {
                      m.values[i][k] = 0;       
                   }
               }
           }
       }
       
       *l_out = l;
       *u_out = m;
       return 0;            
    }
    else {
       return 1;
    }
}

// INVERZNA MATICA POMOCOU LU DEKOMPOZICIE M^-1
int mat_inv(matrix *mp, matrix *inv_out) {
    m = *mp;
    
    if (m.rows == m.columns) {

                //vypocet LU dekompozicie
       (void) ludec(&m, &l, &u);
       
       inv.rows = m.rows;
       inv.columns = m.columns;
       
       z.rows = m.rows;
       z.columns = 1;
       
       x.rows = m.rows;
       x.columns = 1;
       x.values[0][0] = 1; x.values[1][0] = 0; x.values[2][0] = 0;
       
            // vypocet jednotlivych stlpcov inverznej matice
       for (k = m.from[1]; k < m.to[1]; ++k) {
         // vyplnenie vektoru x nulami okrem pozicie k, kde je jednotka
                 for (i = m.from[0]; i < m.to[0]; ++i) {
               if (i == k) {
                  x.values[i][0] = 1;
               }
               else {
                  x.values[i][0] = 0;          
               }
           }
           
                     //riesenie rovnice L * z = x, kde neznamou je vektor z
           for (i = m.from[0]; i < m.to[0]; ++i) {
               sucet = 0;
               for (j = 0; j < i; ++j) {
                   sucet += l.values[i][j]*z.values[j][0];    
               }
               z.values[i][0] = (x.values[i][0] - sucet);
               sucet = 0;
           }
           
                     //riesenie rovnice U * M^-1(:,k) = z, kde neznamou je vektor M^-1(:,k),
                     //co predstavuje k-ty stlpec inverznej matice M
           for (i = m.to[0]- 1; i >= m.from[0]; --i) {
               sucet = 0;
               for (j = m.to[1] - 1 ; j > i; --j) {
                   sucet += u.values[i][j]*inv.values[j][k];    
               }
                                         
                                inv.values[i][k] = (z.values[i][0] - sucet) / u.values[i][i];
                             
               sucet = 0;
           }
       }
       
	   inv.from[0]=0;
	   inv.from[1]=0;
	   inv.to[0]=m.to[0];
	   inv.to[1]=m.to[1];
       *inv_out = inv;        
       return 0;
       }
    else {
       return 1;          
    }
}


// TRANSPOZICIA MATICE M'
int mat_tran(matrix *mp, matrix *tran_out) {
   
    m = *mp;
    m.rows=(m.to[0]-m.from[0]);
	m.columns=(m.to[1]-m.from[1]);
    v.columns = m.rows;
    v.rows =   m.columns;


    for (i = 0; i < m.rows; ++i) {
        for (j = 0; j < m.columns; ++j) {
            v.values[j][i] = m.values[i+m.from[0]][j+m.from[1]];
        }
    }
    v.from[0]=0;
	v.from[1]=0;
	v.to[0]=m.columns;
	v.to[1]=m.rows;
    *tran_out = v;
    return 0;    
}

//JEDEN RIADOK MATICE M(row,:)
int mat_row(matrix *mp, matrix *row_out, int row) {
    m = *mp;
    
    v.rows = 1;
    v.columns = m.columns;
    
    for (i = m.from[1]; i < m.to[1]; ++i) {
        v.values[0][i] = m.values[row][i];
    }
    
    *row_out = v;

        return 0;
}
